#include "FivePointAlgo.hpp"
//#include "5point/5point.h"
#include <zMat.hpp>
#include "../VisionTools.hpp"
#include "Homography.hpp"
#include <Math/Poly3.hpp>
#include <Math/Matrix.hpp>

namespace zzz{

void computeNullspaceBasis(const vector<Vector2d> &ls, const vector<Vector2d> &rs, Matrix<4,9,double> &basis) 
{
  zMatrix<double> A(ls.size(),9),U,S,VT;

  /* Create the 5x9 epipolar constraint matrix */
  for (zuint i = 0; i < ls.size(); i++) 
  {
    A(i,0) = rs[i][0] * ls[i][0];
    A(i,1) = rs[i][1] * ls[i][0];
    A(i,2) = ls[i][0];

    A(i,3) = rs[i][0] * ls[i][1];
    A(i,4) = rs[i][1] * ls[i][1];
    A(i,5) = ls[i][1];

    A(i,6) = rs[i][0];
    A(i,7) = rs[i][1];
    A(i,8) = 1;
  }

  /* Find four vectors that span the right nullspace of the matrix */
  SVD(A,U,S,VT);

  Dress(basis)=VT(Colon(5,8),Colon());
}

void compute_constraint_matrix(const Matrix<4,9,double> &basis, Poly3<double> constraints[10])
{
  /* Basis rows are X, Y, Z, W 
  * Essential matrix is or form x*X + y*Y + z*Z + W */

  /* Create a polynomial for each entry of E */
  Poly3<double> polys[9];

  for (int i=0; i < 9; i++) 
  {
    polys[i][POLY3_X]=basis(0,i);
    polys[i][POLY3_Y]=basis(1,i);
    polys[i][POLY3_Z]=basis(2,i);
    polys[i][POLY3_UNIT]=basis(3,i);
  }

  /* Create a polynormial from the constraint det(E) = 0 */
  Poly3<double> poly_term1 (polys[1].Multiply11(polys[5]) - polys[2].Multiply11(polys[4]));
  poly_term1=poly_term1.Multiply21(polys[6]);

  // matrix_print(1, 20, poly_term1.v);

  Poly3<double> poly_term2 (polys[2].Multiply11(polys[3]) - polys[0].Multiply11(polys[5]));
  poly_term2=poly_term2.Multiply21(polys[7]);

  Poly3<double> poly_term3 (polys[0].Multiply11(polys[4]) - polys[1].Multiply11(polys[3]));
  poly_term3=poly_term3.Multiply21(polys[8]);

  Poly3<double> poly_det = poly_term1 + poly_term2 + poly_term3;

  /* Create polynomials for the singular value constraint */
  Poly3<double> poly_EET[6];
  for (int i = 0; i < 6; i++) {
    int r = 0, c = 0, k;
    switch(i) 
    {
    case 0:
    case 1:
    case 2:
      r = 0;
      c = i;
      break;
    case 3:
    case 4:
      r = 1;
      c = i-2;
      break;
    case 5:
      r = 2;
      c = 2;
      break;
    }

    for (k = 0; k < 3; k++) 
    {
      poly_EET[i] = poly_EET[i] + polys[r*3+k].Multiply11(polys[c*3+k]);
    }
  }

  Poly3<double> poly_tr = (poly_EET[0] + poly_EET[3] + poly_EET[5]) * 0.5;

  Poly3<double> poly_lambda0(poly_EET[0] - poly_tr);
  Poly3<double> poly_lambda1(poly_EET[1]);
  Poly3<double> poly_lambda2(poly_EET[2]);
  Poly3<double> poly_lambda3(poly_EET[3] - poly_tr);
  Poly3<double> poly_lambda4(poly_EET[4]);
  Poly3<double> poly_lambda5(poly_EET[5] - poly_tr);

  constraints[0] = poly_lambda0.Multiply(polys[0]) +\
    poly_lambda1.Multiply(polys[3]) +\
    poly_lambda2.Multiply(polys[6]);

  constraints[1] = poly_lambda0.Multiply(polys[1]) +\
    poly_lambda1.Multiply(polys[4]) +\
    poly_lambda2.Multiply(polys[7]);

  constraints[2] = poly_lambda0.Multiply(polys[2]) +\
    poly_lambda1.Multiply(polys[5]) +\
    poly_lambda2.Multiply(polys[8]);

  constraints[3] = poly_lambda1.Multiply21(polys[0]) +\
    poly_lambda3.Multiply21(polys[3]) +\
    poly_lambda4.Multiply21(polys[6]);

  constraints[4] = poly_lambda1.Multiply21(polys[1]) +\
    poly_lambda3.Multiply21(polys[4]) +\
    poly_lambda4.Multiply21(polys[7]);

  constraints[5] = poly_lambda1.Multiply21(polys[2]) +\
    poly_lambda3.Multiply21(polys[5]) +\
    poly_lambda4.Multiply21(polys[8]);

  constraints[6] = poly_lambda2.Multiply21(polys[0]) +\
    poly_lambda4.Multiply21(polys[3]) +\
    poly_lambda5.Multiply21(polys[6]);

  constraints[7] = poly_lambda2.Multiply21(polys[1]) +\
    poly_lambda4.Multiply21(polys[4]) +\
    poly_lambda5.Multiply21(polys[7]);

  constraints[8] = poly_lambda2.Multiply21(polys[2]) +\
    poly_lambda4.Multiply21(polys[5]) +\
    poly_lambda5.Multiply21(polys[8]);

  constraints[9] = poly_det;
}

void eliminate_gauss_jordan(Poly3<double> constraints[10])
{
  Matrix<10,20,double> A;
  int i, j;

  for (int i = 0; i < 10; i++) 
  {
    Vector<20,double> &row=A.Row(i);
    row[0] = constraints[i][POLY3_X3];
    row[1] = constraints[i][POLY3_Y3];
    row[2] = constraints[i][POLY3_X2Y];
    row[3] = constraints[i][POLY3_XY2];
    row[4] = constraints[i][POLY3_X2Z];
    row[5] = constraints[i][POLY3_X2];
    row[6] = constraints[i][POLY3_Y2Z];
    row[7] = constraints[i][POLY3_Y2];
    row[8] = constraints[i][POLY3_XYZ];
    row[9] = constraints[i][POLY3_XY];
    row[10] = constraints[i][POLY3_XZ2];
    row[11] = constraints[i][POLY3_XZ];
    row[12] = constraints[i][POLY3_X];
    row[13] = constraints[i][POLY3_YZ2];
    row[14] = constraints[i][POLY3_YZ];
    row[15] = constraints[i][POLY3_Y];
    row[16] = constraints[i][POLY3_Z3];
    row[17] = constraints[i][POLY3_Z2];
    row[18] = constraints[i][POLY3_Z];
    row[19] = constraints[i][POLY3_UNIT];
  }

  for (i = 0; i < 10; i++) 
  {
    /* Make the leading coefficient of row i = 1 */
    A.Row(i) *= (1.0/A(i,i));

    /* Subtract from other rows */
    for (j = i+1; j < 10; j++) 
      A.Row(j)-= A.Row(i)*A(j,i);
  }

  /* Now, do the back substitution (stopping four rows early) */
  for (i = 9; i >= 4; i--) 
  {
    for (j = 0; j < i; j++) 
      A.Row(j) -= A.Row(i)*A(j,i);
  }

  /* Copy out results */
  for (i = 0; i < 10; i++) {
    // memcpy(constraints[i].v, A + 20 * i, sizeof(double) * 20);
    Vector<20,double> &row = A.Row(i);
    constraints[i][POLY3_X3] = row[0];
    constraints[i][POLY3_Y3] = row[1];
    constraints[i][POLY3_X2Y] = row[2];
    constraints[i][POLY3_XY2] = row[3];
    constraints[i][POLY3_X2Z] = row[4];
    constraints[i][POLY3_X2] = row[5];
    constraints[i][POLY3_Y2Z] = row[6];
    constraints[i][POLY3_Y2] = row[7];
    constraints[i][POLY3_XYZ] = row[8];
    constraints[i][POLY3_XY] = row[9];
    constraints[i][POLY3_XZ2] = row[10];
    constraints[i][POLY3_XZ] = row[11];
    constraints[i][POLY3_X] = row[12];
    constraints[i][POLY3_YZ2] = row[13];
    constraints[i][POLY3_YZ] = row[14];
    constraints[i][POLY3_Y] = row[15];
    constraints[i][POLY3_Z3] = row[16];
    constraints[i][POLY3_Z2] = row[17];
    constraints[i][POLY3_Z] = row[18];
    constraints[i][POLY3_UNIT] = row[19];
  }
}

void compute_B_matrix(Poly3<double> constraints[10], Matrix<9,11,double> &B) 
{
  Poly3<double> e = constraints[4];
  Poly3<double> f = constraints[5];
  Poly3<double> g = constraints[6];
  Poly3<double> h = constraints[7];
  Poly3<double> i = constraints[8];
  Poly3<double> j = constraints[9];

  B.Zero();

  B(0,3) = -f[POLY3_XZ2];
  B(0,2) = e[POLY3_XZ2] - f[POLY3_XZ];
  B(0,1) = e[POLY3_XZ] - f[POLY3_X];
  B(0,0) = e[POLY3_X];

  B(1,3) = -f[POLY3_YZ2];
  B(1,2) = e[POLY3_YZ2] - f[POLY3_YZ];
  B(1,1) = e[POLY3_YZ] - f[POLY3_Y];
  B(1,0) = e[POLY3_Y];

  B(2,4) = -f[POLY3_Z3];
  B(2,3) = e[POLY3_Z3] - f[POLY3_Z2];
  B(2,2) = e[POLY3_Z2] - f[POLY3_Z];
  B(2,1) = e[POLY3_Z] - f[POLY3_UNIT];
  B(2,0) = e[POLY3_UNIT];

  B(3,3) = -h[POLY3_XZ2];
  B(3,2) = g[POLY3_XZ2] - h[POLY3_XZ];
  B(3,1) = g[POLY3_XZ] - h[POLY3_X];
  B(3,0) = g[POLY3_X];

  B(4,3) = -h[POLY3_YZ2];
  B(4,2) = g[POLY3_YZ2] - h[POLY3_YZ];
  B(4,1) = g[POLY3_YZ] - h[POLY3_Y];
  B(4,0) = g[POLY3_Y];

  B(5,4) = -h[POLY3_Z3];
  B(5,3) = g[POLY3_Z3] - h[POLY3_Z2];
  B(5,2) = g[POLY3_Z2] - h[POLY3_Z];
  B(5,1) = g[POLY3_Z] - h[POLY3_UNIT];
  B(5,0) = g[POLY3_UNIT];

  B(6,3) = -j[POLY3_XZ2];
  B(6,2) = i[POLY3_XZ2] - j[POLY3_XZ];
  B(6,1) = i[POLY3_XZ] - j[POLY3_X];
  B(6,0) = i[POLY3_X];

  B(7,3) = -j[POLY3_YZ2];
  B(7,2) = i[POLY3_YZ2] - j[POLY3_YZ];
  B(7,1) = i[POLY3_YZ] - j[POLY3_Y];
  B(7,0) = i[POLY3_Y];

  B(8,4) = -j[POLY3_Z3];
  B(8,3) = i[POLY3_Z3] - j[POLY3_Z2];
  B(8,2) = i[POLY3_Z2] - j[POLY3_Z];
  B(8,1) = i[POLY3_Z] - j[POLY3_UNIT];
  B(8,0) = i[POLY3_UNIT];
}

Vector<11,double> poly1_mult(const Vector<11,double> &a, const Vector<11,double> &b) 
{
  Vector<11,double> r(0);
  for (int i = 0; i <= 10; i++) for (int j = 0; j <= 10; j++) 
  {
    int place = i + j;
    if (place > 10) continue;
    r[place] += a[i] * b[j];
  }
  return r;
}

void compute_determinant(Matrix<9,11,double> B, Vector<11,double> &p1, Vector<11,double> &p2, Vector<11,double> &p3, Vector<11,double> &det)
{
  p1 = poly1_mult(B.Row(1), B.Row(5)) - poly1_mult(B.Row(2), B.Row(4));
  p2 = poly1_mult(B.Row(2), B.Row(3)) - poly1_mult(B.Row(0), B.Row(5));
  p3 = poly1_mult(B.Row(0), B.Row(4)) - poly1_mult(B.Row(1), B.Row(3));
  det = poly1_mult(p1, B.Row(6)) + poly1_mult(p2, B.Row(7)) + poly1_mult(p3, B.Row(8));
}

int poly1_degree(const Vector<11,double> &a) 
{
  for (int i = 10; i >= 0; i--)
    if (fabs(a[i]) > 0.0) return i;
  return 0;
}

Vector<11,double> poly1_normalize(const Vector<11,double> &a) 
{
  int d = poly1_degree(a);
  if (a[d] != 0) return a * (1.0 / a[d]);
  else return a;
}

void extract_roots(const Vector<11,double> &det, int *nuroots_, Vector<10,double> &roots)
{
  zMatrix<double,zColMajor> C(10,10), evec, eval, evali;
  C.Zero();

  /* Scale the determinant */
  Vector<11,double> det_scale = poly1_normalize(det);

  /* Fill the companion matrix */
  for (int i = 0; i < 10; i++) C(0,i) = -det_scale[9-i];
  //C(0,Colon())=-Dress(det_scale)(Colon(9,0,-1));

  for (int i = 1; i < 10; i++) C(i,i-1) = 1.0;

  EigRight(C, evec, eval, evali);

  //take those roots only have real part
  *nuroots_=0;
  for (zuint i=0; i<10; i++)
  {
    if (evali[i]==0.0)
    {
      roots[*nuroots_]=eval[i];
      (*nuroots_)++;
    }
  }
}

double poly1_eval(Vector<11,double> &a, double x) 
{
  double p = 1.0;
  double r = 0.0;
  for (int i = 0; i <= 10; i++) 
  {
    r += p * a[i];
    p = p * x;
  }
  return r;
}

void compute_Ematrices(int nuposes_, Vector<10,double> &roots, const Matrix<4,9,double> &basis, 
  Vector<11,double> &p1, Vector<11,double> &p2, Vector<11,double> &p3,  Vector<10,EssentialMat> &E) 
{
  for (int i = 0; i < nuposes_; i++) 
  {
    double z = roots[i];
    double den = poly1_eval(p3, z);
    double den_inv = 1.0 / den;

    double x = poly1_eval(p1, z) * den_inv;
    double y = poly1_eval(p2, z) * den_inv;

    Vector<9,double> X=basis.Row(0)*x;
    Vector<9,double> Y=basis.Row(1)*y;
    Vector<9,double> Z=basis.Row(2)*z;

    Vector<9,double> tmp=X+Y+Z+basis.Row(3);
    memcpy(E[i].Data(),tmp.Data(),sizeof(double)*9);
  }
}

void compute_Grabner_basis(const Poly3<double> constraints[10], Matrix<10,10,double> &Gbasis) 
{
  Matrix<10,20,double> A;
  int i, j;

  for (i = 0; i < 10; i++) {
    // memcpy(A + 20 * i, constraints[i].v, sizeof(double) * 20);
    Vector<20,double> &row = A.Row(i);

    /* x3 x2y xy2 y3 x2z xyz y2z xz2 yz2 z3 x2 xy y2 xz yz z2 x  y  z  1 */

    row[0] = constraints[i][POLY3_X3];
    row[1] = constraints[i][POLY3_X2Y];
    row[2] = constraints[i][POLY3_XY2];
    row[3] = constraints[i][POLY3_Y3];
    row[4] = constraints[i][POLY3_X2Z];
    row[5] = constraints[i][POLY3_XYZ];
    row[6] = constraints[i][POLY3_Y2Z];
    row[7] = constraints[i][POLY3_XZ2];
    row[8] = constraints[i][POLY3_YZ2];
    row[9] = constraints[i][POLY3_Z3];
    row[10] = constraints[i][POLY3_X2];
    row[11] = constraints[i][POLY3_XY];
    row[12] = constraints[i][POLY3_Y2];
    row[13] = constraints[i][POLY3_XZ];
    row[14] = constraints[i][POLY3_YZ];
    row[15] = constraints[i][POLY3_Z2];
    row[16] = constraints[i][POLY3_X];
    row[17] = constraints[i][POLY3_Y];
    row[18] = constraints[i][POLY3_Z];
    row[19] = constraints[i][POLY3_UNIT];
  }

  /* Do a full Gaussian elimination */

  for (i = 0; i < 10; i++) 
  {
    /* Make the leading coefficient of row i = 1 */
    double leading = A(i,i);
    A.Row(i) *= (1.0/leading);

    /* Subtract from other rows */
    for (j = i+1; j < 10; j++) 
    {
      double leading2 = A(j,i);
      Vector<20,double> scaled_row=A.Row(i)*leading2;
      A.Row(j)-=scaled_row;
    }
  }

  /* Now, do the back substitution */
  for (i = 9; i >= 0; i--) 
  {
    for (j = 0; j < i; j++) 
    {
      double scale = A(j,i);
      Vector<20,double> scaled_row=A.Row(i)*scale;
      A.Row(j)-=scaled_row;
    }
  }

  /* Copy out results */
  for (i = 0; i < 10; i++) 
  {
    memcpy(Gbasis.Row(i).Data(), &(A.Row(i)[10]), sizeof(double) * 10);
  }
}

void compute_action_matrix(const Matrix<10,10,double> &Gbasis, Matrix<10,10,double> &At) 
{
  At.Zero();

  At.Row(0) = -Gbasis.Row(0);
  At.Row(1) = -Gbasis.Row(1);
  At.Row(2) = -Gbasis.Row(2);
  At.Row(3) = -Gbasis.Row(4);
  At.Row(4) = -Gbasis.Row(5);
  At.Row(5) = -Gbasis.Row(7);

  At(6,0) = 1.0;
  At(7,1) = 1.0;
  At(8,3) = 1.0;
  At(9,6) = 1.0;
}

void compute_Ematrices_Gb(const Matrix<10,10,double> &At, const Matrix<4,9,double> &basis, int *nusolns_, Vector<10,EssentialMat> &E)
{
  zMatrix<double,zColMajor> A(Dress(At)), evec, eval, evali;
  EigRight(A, evec, eval, evali);

  *nusolns_=0;
  for (int i = 0; i < 10; i++) 
  {
    if (evali[i]==0.0)
    {

      double x = evec(6,i);
      double y = evec(7,i);
      double z = evec(8,i);
      double w = evec(9,i);
      double w_inv = 1.0 / w;

      x = x * w_inv;
      y = y * w_inv;
      z = z * w_inv;

      Vector<9,double> X = basis.Row(0)*x;
      Vector<9,double> Y = basis.Row(1)*y;
      Vector<9,double> Z = basis.Row(2)*z;

      Vector<9,double> tmp=X+Y+Z+basis.Row(3);
      memcpy(E[(*nusolns_)].Data(),tmp.Data(),sizeof(double)*9);

      E[(*nusolns_)]/=E[(*nusolns_)](2,2);
    
      (*nusolns_)++;
    }
  }
}

void GenerateEmatrixHypotheses(const vector<Vector2d> &ls, const vector<Vector2d> &rs, Vector<10,EssentialMat> &E, int *nuposes_)
{
  Matrix<4,9,double> basis;
  computeNullspaceBasis(ls, rs, basis);
  Poly3<double> constraints[10];
  compute_constraint_matrix(basis, constraints);
#define FIVE_POINT_OPT
#ifndef FIVE_POINT_OPT
  eliminate_gauss_jordan(constraints);

  Matrix<9,11,double> B;
  compute_B_matrix(constraints, B);

  Vector<11,double> p1,p2,p3,det;
  compute_determinant(B, p1, p2, p3, det);

  Vector<10,double> roots;
  extract_roots(det, nuposes_, roots);
  compute_Ematrices(*nuposes_, roots, basis, p1, p2, p3, E);
#else
  Matrix<10,10,double> Gbasis;
  compute_Grabner_basis(constraints, Gbasis);

  Matrix<10,10,double> At;
  compute_action_matrix(Gbasis, At);

  compute_Ematrices_Gb(At, basis, nuposes_, E);
#endif
}


int EvaluateEmatrix(const vector<Vector2d> &rs, const vector<Vector2d> &ls, double thresh_norm, const EssentialMat &F, int *best_inlier, double *score)
{
  int nuinliers_ = 0;
  double min_resid = 1.0e20;
  double likelihood = 0.0;

  zuint n=rs.size();
  for (zuint i = 0; i < n; i++)
  {
    double resid=F.Residue(ls[i],rs[i]);

    likelihood += log(1.0 + resid * resid / (thresh_norm));

    if (resid < thresh_norm)
    {
      nuinliers_++;
      if (resid < min_resid) 
      {
        min_resid = resid;
        *best_inlier = i;
      }
    }
  }

  *score = likelihood;
  // *score = 1.0 / nuinliers_;

  return nuinliers_;
}

int FivePointAlgo(EssentialMat &E, const Matrix3x3d &K, const vector<Vector2d> &ls, const vector<Vector2d> &rs, const double outlier_shreshold, IterExitCond<double> &cond)
{
  //implementation of "An Efficient Solution to the Five-Point Relative Pose Problem" by David Nister

  //essential matrix and new point correspondence
  Matrix3x3d K_inv=K.Inverted();

  //RANSAC, ramdonly pick 5 point to do five-point algorithm
  double thresh_norm=outlier_shreshold*outlier_shreshold;
  cond.Reset();

  int max_inliers=0;
  double min_score=MAX_FLOAT;
  EssentialMat E_best;

  typedef RANSACPicker<5> PickerType;
  PickerType picker(0,ls.size()-1);
  picker.SeedFromTime();
  do
  {
    //randomly pick 5 points
    PickerType::PickType pick=picker.Pick();

    //evaluate these 5 points, so they should not make a homography
    {
      vector<Vector2d> ls_homo,rs_homo;
      for (zuint i=0; i<pick.size(); i++)
      {
        ls_homo.push_back(ls[pick[i]]);
        rs_homo.push_back(rs[pick[i]]);
      }
      Homography homography;
      homography.Create(ls_homo,rs_homo);
      double homo_error=0;
      for (zuint i=0; i<rs_homo.size(); i++)
        homo_error += rs_homo[i].DistTo(FromHomogeneous(homography.GetHab()*ToHomogeneous(ls_homo[i])));
      if (homo_error<10) {
        ZLOGE<<"homo_error:"<<homo_error<<endl;
        continue;
      }
    }

    //normalize to calculate E
    vector<Vector2d> ls_norm,rs_norm;
    for (zuint i=0; i<pick.size(); i++)
    {
      ls_norm.push_back(FromHomogeneous(K_inv*ToHomogeneous(ls[pick[i]])));
      rs_norm.push_back(FromHomogeneous(K_inv*ToHomogeneous(rs[pick[i]])));
    }

    //five-point algorithm
    Vector<10,EssentialMat> E;
    int nuhyp_;

//      ZDEBUG(SplitLine("mine"));
    GenerateEmatrixHypotheses(ls_norm,rs_norm,E,&nuhyp_);
    
    //DEBUG
/*      ZDEBUG("Num:"+nuhyp_);
    for (int i=0; i<nuhyp_; i++)
      ZDEBUG(string("E")<<i<<"=\n"<<E[i]);

    zout<<SplitLine("theirs");

    Vector<10,EssentialMat> E2;
    int nuhyp2_;
    v2_t *rt_pts=new v2_t[lls.size()];
    v2_t *lt_pts=new v2_t[lls.size()];
    memcpy(lt_pts,lls[0].Data(),sizeof(double)*2*lls.size());
    memcpy(rt_pts,rrs[0].Data(),sizeof(double)*2*rrs.size());
    generate_Ematrix_hypotheses(lls.size(),rt_pts,lt_pts,&nuhyp2_,(double*)E2.Data());
    delete[] lt_pts;
    delete[] rt_pts;

    ZDEBUG("Num:"+nuhyp2_);
    for (int i=0; i<nuhyp2_; i++)
      ZDEBUG(string("E")<<i<<"=\n"<<E2[i]);
*/      //END OF DEBUG

    int best=0;
    int inliers_hyp[10];
    for (int i = 0; i < nuhyp_; i++) 
    {
      int best_inlier;
      double score = 0.0;

      EssentialMat F=K_inv.Transposed()*E[i]*K_inv;  //convert back
      //F/=F(2,2);
      F.Normalize();

      int nuinliers_ = EvaluateEmatrix(rs, ls, outlier_shreshold, F, &best_inlier, &score);

      if (nuinliers_ > max_inliers || (nuinliers_ == max_inliers && score < min_score)) 
      {
        best = 1;
        max_inliers = nuinliers_;
        min_score = score;
        E_best=E[i];

        ZLOG(ZDEBUG)<<SplitLine("E")<<endl \
          <<"E=\n"<<E_best<<endl \
          <<"nuinliers_:"<<nuinliers_<<endl;
      }

      inliers_hyp[i] = nuinliers_;
    }
  
  }while(!cond.IsSatisfied(double(ls.size()-max_inliers)/ls.size()));

  E=E_best/E_best(2,2);
  ZLOG(ZVERBOSE)<<"best_E=\n"<<E \
    <<"max_inliers=\n"<<max_inliers<<endl;
  
  return max_inliers;
}



}

